class PointCloudViewer {
    constructor() {
        this.scene = null;
        this.camera = null;
        this.renderer = null;
        this.controls = null;
        this.pointCloud = null;
        this.ws = null;

        this.pointSize = 1.0;
        this.colorMode = 'height';
        this.maxPoints = 100000; // 最大点数限制

        this.frameCount = 0;
        this.lastTime = Date.now();

        this.init();
        this.setupWebSocket();
        this.setupControls();
        this.animate();
    }

    init() {
        const container = document.getElementById('container');

        // 创建场景
        this.scene = new THREE.Scene();
        this.scene.background = new THREE.Color(0x202020);

        // 创建相机
        this.camera = new THREE.PerspectiveCamera(
            75,
            window.innerWidth / window.innerHeight,
            0.1,
            1000
        );
        this.camera.position.set(10, 10, 10);

        // 创建渲染器
        this.renderer = new THREE.WebGLRenderer({ antialias: true });
        this.renderer.setSize(window.innerWidth, window.innerHeight);
        container.appendChild(this.renderer.domElement);

        // 创建控制器
        this.controls = new THREE.OrbitControls(this.camera, this.renderer.domElement);
        this.controls.enableDamping = true;
        this.controls.dampingFactor = 0.1;

        // 添加坐标轴辅助器
        const axesHelper = new THREE.AxesHelper(5);
        this.scene.add(axesHelper);

        // 添加网格
        const gridHelper = new THREE.GridHelper(20, 20);
        gridHelper.position.y = -2;
        this.scene.add(gridHelper);

        // 处理窗口大小变化
        window.addEventListener('resize', () => this.onWindowResize());
    }

    setupWebSocket() {
        const wsUrl = `ws://localhost:8080`;
        this.ws = new WebSocket(wsUrl);

        this.ws.binaryType = 'arraybuffer';

        this.ws.onopen = () => {
            console.log('WebSocket connected');
            this.updateStatus('Connected');
        };

        this.ws.onmessage = (event) => {
            this.handlePointCloudData(event.data);
        };

        this.ws.onclose = () => {
            console.log('WebSocket disconnected');
            this.updateStatus('Disconnected');
            // 自动重连
            setTimeout(() => this.setupWebSocket(), 2000);
        };

        this.ws.onerror = (error) => {
            console.error('WebSocket error:', error);
            this.updateStatus('Error');
        };
    }

    handlePointCloudData(arrayBuffer) {
    try {
        console.log('Processing arrayBuffer:', arrayBuffer.byteLength, 'bytes');
        
        const data = new Float32Array(arrayBuffer);
        const numPoints = data.length / 4;

        console.log(`Received ${numPoints} points, total floats: ${data.length}`);
        
        if (numPoints === 0) {
            console.warn('Received empty point cloud');
            return;
        }
        
        if (!isFinite(numPoints)) {
            console.error('Invalid number of points:', numPoints);
            return;
        }

        // 验证数据完整性
        let nanCount = 0;
        let infCount = 0;
        let validCount = 0;
        
        for (let i = 0; i < data.length; i++) {
            if (isNaN(data[i])) {
                nanCount++;
            } else if (!isFinite(data[i])) {
                infCount++;
            } else {
                validCount++;
            }
        }
        
        console.log(`Data validation: ${validCount} valid, ${nanCount} NaN, ${infCount} infinite values`);
        
        if (validCount === 0) {
            console.error('No valid data found');
            return;
        }

        // 检查前几个点的数据
        console.log('First 3 points (raw):');
        for (let i = 0; i < Math.min(3, numPoints); i++) {
            const x = data[i * 4];
            const y = data[i * 4 + 1];
            const z = data[i * 4 + 2];
            const intensity = data[i * 4 + 3];
            console.log(`Point ${i}: (${x}, ${y}, ${z}, ${intensity})`);
            console.log(`  Valid: x=${isFinite(x)}, y=${isFinite(y)}, z=${isFinite(z)}, i=${isFinite(intensity)}`);
        }

        const actualPoints = Math.min(numPoints, this.maxPoints);

        // 保存数据
        this.lastFrameData = data;
        this.lastNumPoints = actualPoints;

        this.updatePointCloud(data, actualPoints);
        this.updatePointCount(numPoints);

    } catch (error) {
        console.error('Error processing point cloud data:', error);
    }
}


    updatePointCloud(data, numPoints) {
    console.log(`Updating point cloud with ${numPoints} points`);
    
    // 移除旧的点云
    if (this.pointCloud) {
        this.scene.remove(this.pointCloud);
        this.pointCloud.geometry.dispose();
        this.pointCloud.material.dispose();
    }

    // 创建几何体
    const geometry = new THREE.BufferGeometry();
    
    // 预分配数组
    const maxValidPoints = numPoints; // 先分配最大空间
    const tempPositions = new Float32Array(maxValidPoints * 3);
    const tempColors = new Float32Array(maxValidPoints * 3);

    let minZ = Infinity, maxZ = -Infinity;
    let minIntensity = Infinity, maxIntensity = -Infinity;
    let minX = Infinity, maxX = -Infinity;
    let minY = Infinity, maxY = -Infinity;

    const coordinateSystem = this.coordinateSystem || 'ROS_TO_THREEJS';

    // 第一遍：验证数据并收集统计信息
    let validPointCount = 0;
    const validPoints = []; // 存储有效的点

    for (let i = 0; i < numPoints; i++) {
        let x = data[i * 4];
        let y = data[i * 4 + 1];
        let z = data[i * 4 + 2];
        const intensity = data[i * 4 + 3];

        // 严格的数据验证
        if (!isFinite(x) || !isFinite(y) || !isFinite(z) || !isFinite(intensity)) {
            if (validPointCount < 10) { // 只打印前10个无效点
                console.warn(`Invalid point at index ${i}: (${x}, ${y}, ${z}, ${intensity})`);
            }
            continue;
        }

        // 检查是否是异常大的值
        const maxCoord = 1000; // 根据你的数据调整这个阈值
        if (Math.abs(x) > maxCoord || Math.abs(y) > maxCoord || Math.abs(z) > maxCoord) {
            if (validPointCount < 10) {
                console.warn(`Point with large coordinates at index ${i}: (${x}, ${y}, ${z})`);
            }
            continue;
        }

        // 坐标系转换
        let transformedPos;
        try {
            transformedPos = this.transformCoordinates(x, y, z, coordinateSystem);
            
            // 验证转换后的坐标
            if (!isFinite(transformedPos.x) || !isFinite(transformedPos.y) || !isFinite(transformedPos.z)) {
                console.warn(`Transform resulted in invalid coordinates for point ${i}`);
                continue;
            }
            
        } catch (error) {
            console.warn(`Transform error for point ${i}:`, error);
            continue;
        }

        // 存储有效点
        validPoints.push({
            original: { x, y, z, intensity },
            transformed: transformedPos,
            intensity: intensity
        });

        // 更新边界
        minX = Math.min(minX, transformedPos.x);
        maxX = Math.max(maxX, transformedPos.x);
        minY = Math.min(minY, transformedPos.y);
        maxY = Math.max(maxY, transformedPos.y);
        minZ = Math.min(minZ, transformedPos.z);
        maxZ = Math.max(maxZ, transformedPos.z);
        minIntensity = Math.min(minIntensity, intensity);
        maxIntensity = Math.max(maxIntensity, intensity);
        
        validPointCount++;
    }

    console.log(`Found ${validPointCount} valid points out of ${numPoints}`);

    if (validPointCount === 0) {
        console.warn('No valid points to render');
        return;
    }

    // 检查边界是否有效
    if (!isFinite(minX) || !isFinite(maxX) || !isFinite(minY) || !isFinite(maxY) || 
        !isFinite(minZ) || !isFinite(maxZ)) {
        console.error('Invalid bounds calculated');
        return;
    }

    console.log('Point cloud bounds:');
    console.log(`X: ${minX.toFixed(3)} to ${maxX.toFixed(3)}`);
    console.log(`Y: ${minY.toFixed(3)} to ${maxY.toFixed(3)}`);
    console.log(`Z: ${minZ.toFixed(3)} to ${maxZ.toFixed(3)}`);
    console.log(`Intensity: ${minIntensity.toFixed(3)} to ${maxIntensity.toFixed(3)}`);

    // 创建最终的数组
    const positions = new Float32Array(validPointCount * 3);
    const colors = new Float32Array(validPointCount * 3);

    // 第二遍：填充位置和颜色数据
    for (let i = 0; i < validPointCount; i++) {
        const point = validPoints[i];
        
        // 设置位置
        positions[i * 3] = point.transformed.x;
        positions[i * 3 + 1] = point.transformed.y;
        positions[i * 3 + 2] = point.transformed.z;

        // 生成颜色
        let color = new THREE.Color();

        switch (this.colorMode) {
            case 'height':
                const heightRange = maxZ - minZ;
                if (heightRange === 0) {
                    color.setRGB(0.5, 0.5, 0.5); // 灰色用于单一高度
                } else {
                    const heightRatio = (point.transformed.z - minZ) / heightRange;
                    color.setHSL(0.7 - heightRatio * 0.7, 1, 0.5);
                }
                break;

            case 'intensity':
                const intensityRange = maxIntensity - minIntensity;
                if (intensityRange === 0) {
                    color.setRGB(0.5, 0.5, 0.5); // 灰色用于单一强度
                } else {
                    const intensityRatio = (point.intensity - minIntensity) / intensityRange;
                    color.setHSL(0, 0, intensityRatio);
                }
                break;

            case 'fixed':
            default:
                color.setRGB(0.8, 0.8, 0.8);
                break;
        }

        colors[i * 3] = color.r;
        colors[i * 3 + 1] = color.g;
        colors[i * 3 + 2] = color.b;
    }

    // 最终验证
    for (let i = 0; i < positions.length; i++) {
        if (!isFinite(positions[i])) {
            console.error(`NaN found in positions array at index ${i}`);
            positions[i] = 0; // 替换为0
        }
    }

    for (let i = 0; i < colors.length; i++) {
        if (!isFinite(colors[i])) {
            console.error(`NaN found in colors array at index ${i}`);
            colors[i] = 0.5; // 替换为中性色
        }
    }

    geometry.setAttribute('position', new THREE.BufferAttribute(positions, 3));
    geometry.setAttribute('color', new THREE.BufferAttribute(colors, 3));

    // 手动计算边界框以避免NaN
    const bbox = new THREE.Box3();
    bbox.setFromArray(positions);
    
    if (bbox.isEmpty() || !isFinite(bbox.min.x) || !isFinite(bbox.max.x)) {
        console.error('Invalid bounding box, using default');
        geometry.boundingBox = new THREE.Box3(
            new THREE.Vector3(-1, -1, -1),
            new THREE.Vector3(1, 1, 1)
        );
    } else {
        geometry.boundingBox = bbox;
    }

    console.log('Final geometry bounding box:', geometry.boundingBox);

    // 创建材质
    const material = new THREE.PointsMaterial({
        size: this.pointSize,
        vertexColors: true,
        sizeAttenuation: false
    });

    // 创建点云对象
    this.pointCloud = new THREE.Points(geometry, material);
    this.scene.add(this.pointCloud);

    console.log('Point cloud added to scene successfully');

    // 自动调整视角
    if (validPointCount > 0 && (this.isFirstFrame || this.autoResetView)) {
        this.adjustCamera(geometry.boundingBox);
    }
}
adjustCamera(boundingBox) {
    try {
        const center = boundingBox.getCenter(new THREE.Vector3());
        const size = boundingBox.getSize(new THREE.Vector3());
        
        // 验证中心点和尺寸
        if (!isFinite(center.x) || !isFinite(center.y) || !isFinite(center.z)) {
            console.warn('Invalid center, using origin');
            center.set(0, 0, 0);
        }
        
        if (!isFinite(size.x) || !isFinite(size.y) || !isFinite(size.z)) {
            console.warn('Invalid size, using default');
            size.set(10, 10, 10);
        }
        
        const maxDim = Math.max(size.x, size.y, size.z, 1); // 确保不为0

        console.log('Adjusting camera view:');
        console.log('Center:', center);
        console.log('Size:', size);
        console.log('Max dimension:', maxDim);

        if (this.isFirstFrame || this.autoResetView) {
            this.camera.position.set(
                center.x + maxDim * 2,
                center.y + maxDim * 2,
                center.z + maxDim * 2
            );
            this.controls.target.copy(center);
            this.controls.update();

            this.isFirstFrame = false;
            console.log('Camera positioned at:', this.camera.position);
        }
    } catch (error) {
        console.error('Error adjusting camera:', error);
    }
}
// // 改进的坐标转换函数
// transformCoordinates(x, y, z, coordinateSystem) {
//     // 输入验证
//     if (!isFinite(x) || !isFinite(y) || !isFinite(z)) {
//         throw new Error(`Invalid input coordinates: (${x}, ${y}, ${z})`);
//     }

//     switch (coordinateSystem) {
//         case 'ROS_TO_THREEJS':
//             return {
//                 x: -y,  // ROS Y -> THREE.js -X
//                 y: z,   // ROS Z -> THREE.js Y  
//                 z: x    // ROS X -> THREE.js Z
//             };
//         case 'IDENTITY':
//             return { x, y, z };
//         default:
//             return { x, y, z };
//     }
// }
    // 添加坐标转换方法
    transformCoordinates(x, y, z, system) {
        switch (system) {
            case 'ROS_TO_THREEJS':
                // ROS坐标系 (x前, y左, z上) 转 Three.js坐标系 (x右, y上, z前)
                return {
                    x: -y,  // ROS的-y -> Three.js的x
                    y: z,   // ROS的z -> Three.js的y  
                    z: -x   // ROS的-x -> Three.js的z
                };

            case 'VELODYNE_TO_THREEJS':
                // Velodyne坐标系 (x前, y左, z上) 转 Three.js坐标系
                return {
                    x: y,   // Velodyne的y -> Three.js的x
                    y: z,   // Velodyne的z -> Three.js的y
                    z: x    // Velodyne的x -> Three.js的z
                };

            case 'ROTATE_Z_90':
                // 绕Z轴旋转90度
                return {
                    x: -y,
                    y: x,
                    z: z
                };

            case 'ROTATE_Y_90':
                // 绕Y轴旋转90度
                return {
                    x: z,
                    y: y,
                    z: -x
                };

            case 'ROTATE_X_90':
                // 绕X轴旋转90度
                return {
                    x: x,
                    y: -z,
                    z: y
                };

            case 'SWAP_XZ':
                // 交换X和Z轴
                return {
                    x: z,
                    y: y,
                    z: x
                };

            case 'SWAP_YZ':
                // 交换Y和Z轴
                return {
                    x: x,
                    y: z,
                    z: y
                };

            case 'FLIP_Z':
                // 翻转Z轴
                return {
                    x: x,
                    y: y,
                    z: -z
                };

            case 'NONE':
            default:
                // 不转换
                return { x, y, z };
        }
    }


    setupControls() {
        // 点大小控制
        const pointSizeSlider = document.getElementById('pointSize');
        const pointSizeValue = document.getElementById('pointSizeValue');

        pointSizeSlider.addEventListener('input', (e) => {
            this.pointSize = parseFloat(e.target.value);
            pointSizeValue.textContent = this.pointSize.toFixed(1);

            if (this.pointCloud) {
                this.pointCloud.material.size = this.pointSize;
            }
        });

        // 颜色模式控制
        const colorModeSelect = document.getElementById('colorMode');
        colorModeSelect.addEventListener('change', (e) => {
            this.colorMode = e.target.value;
            // 需要重新处理最后一帧数据
            this.reprocessLastFrame();
        });

        // 坐标系切换控制
        const coordinateSystemSelect = document.getElementById('coordinateSystem');
        coordinateSystemSelect.addEventListener('change', (e) => {
            this.coordinateSystem = e.target.value;
            console.log('Coordinate system changed to:', this.coordinateSystem);
            // 需要重新处理最后一帧数据
            this.reprocessLastFrame();
        });
        // 自动重置视角控制
        const autoResetCheckbox = document.getElementById('autoResetView');
        autoResetCheckbox.addEventListener('change', (e) => {
            this.autoResetView = e.target.checked;
            console.log('Auto reset view:', this.autoResetView);
        });
        // 重置视角按钮
        const resetViewBtn = document.getElementById('resetView');
        resetViewBtn.addEventListener('click', () => {
            this.resetCameraView();
        });
        // 适应数据按钮
        const fitViewBtn = document.getElementById('fitView');
        fitViewBtn.addEventListener('click', () => {
            this.fitCameraToData();
        });
    }

    // 添加重新处理最后一帧的方法
    reprocessLastFrame() {
        if (this.lastFrameData && this.lastNumPoints) {
            this.updatePointCloud(this.lastFrameData, this.lastNumPoints);
        }
    }
    // 添加重置相机视角方法
    resetCameraView() {
        this.camera.position.set(10, 10, 10);
        this.controls.target.set(0, 0, 0);
        this.controls.update();
        console.log('Camera view reset to default');
    }

    // 添加适应数据的视角方法
    fitCameraToData() {
        if (!this.pointCloud) {
            console.warn('No point cloud data to fit to');
            return;
        }

        this.pointCloud.geometry.computeBoundingBox();
        const bbox = this.pointCloud.geometry.boundingBox;

        if (!bbox) {
            console.warn('No bounding box available');
            return;
        }

        const center = bbox.getCenter(new THREE.Vector3());
        const size = bbox.getSize(new THREE.Vector3());
        const maxDim = Math.max(size.x, size.y, size.z);

        // 计算合适的相机距离
        const distance = maxDim * 2;
        const direction = this.camera.position.clone().sub(this.controls.target).normalize();

        this.camera.position.copy(center).add(direction.multiplyScalar(distance));
        this.controls.target.copy(center);
        this.controls.update();

        console.log('Camera fitted to point cloud data');
    }
    // 更新相机状态显示
    updateCameraStatus() {
        const statusElement = document.getElementById('cameraStatus');
        if (statusElement) {
            if (this.userInteracting) {
                statusElement.textContent = 'User Control';
            } else if (this.autoResetView) {
                statusElement.textContent = 'Auto Reset';
            } else {
                statusElement.textContent = 'Manual';
            }
        }
    }
    animate() {
        requestAnimationFrame(() => this.animate());

        this.controls.update();
        this.renderer.render(this.scene, this.camera);
        this.updateCameraStatus(); 

        // 更新FPS
        this.frameCount++;
        const now = Date.now();
        if (now - this.lastTime >= 1000) {
            this.updateFPS(this.frameCount);
            this.frameCount = 0;
            this.lastTime = now;
        }
    }

    onWindowResize() {
        this.camera.aspect = window.innerWidth / window.innerHeight;
        this.camera.updateProjectionMatrix();
        this.renderer.setSize(window.innerWidth, window.innerHeight);
    }

    updateStatus(status) {
        document.getElementById('status').textContent = status;
    }

    updatePointCount(count) {
        document.getElementById('pointCount').textContent = count.toLocaleString();
    }

    updateFPS(fps) {
        document.getElementById('fps').textContent = fps;
    }
}

// 初始化点云查看器
window.addEventListener('DOMContentLoaded', () => {
    new PointCloudViewer();
});
